/**
 * @file pid_controller.c
 * @brief 通用PID控制器库实现文件
 */

#include "pid_controller.h"
#include <stdio.h>  // 仅用于调试（如需要）

/**
 * @brief 初始化PID控制器
 */
void PID_Init(
    PIDController *pid,
    float kp, float ki, float kd,
    float setpoint,
    float output_min, float output_max,
    float integral_min, float integral_max,
    int use_integral, int use_derivative
) {
    pid->kp = kp;
    pid->ki = ki;
    pid->kd = kd;
    pid->setpoint = setpoint;
    pid->output_min = output_min;
    pid->output_max = output_max;
    pid->integral_min = integral_min;
    pid->integral_max = integral_max;
    pid->use_integral = use_integral;
    pid->use_derivative = use_derivative;
    PID_Reset(pid);  // 初始化时重置内部状态
}

/**
 * @brief 更新PID控制器并计算输出
 */
float PID_Update(PIDController *pid, float current_value) {
    // 计算误差
    float error = pid->setpoint - current_value;

    // 积分项处理
    if (pid->use_integral) {
        pid->integral += error;
        // 积分抗饱和：限制积分项范围
        if (pid->integral > pid->integral_max) {
            pid->integral = pid->integral_max;
        } else if (pid->integral < pid->integral_min) {
            pid->integral = pid->integral_min;
        }
    } else {
        pid->integral = 0.0;  // 禁用积分项
    }

    // 微分项处理
    float derivative = 0.0;
    if (pid->use_derivative) {
        derivative = error - pid->previous_error;
    }

    // 计算PID输出
    pid->output = (pid->kp * error) + 
                  (pid->ki * pid->integral) + 
                  (pid->kd * derivative);

    // 输出限幅
    if (pid->output > pid->output_max) {
        pid->output = pid->output_max;
    } else if (pid->output < pid->output_min) {
        pid->output = pid->output_min;
    }

    // 保存当前误差供下次微分计算
    pid->previous_error = error;

    return pid->output;
}

/**
 * @brief 重置PID控制器的内部状态
 */
void PID_Reset(PIDController *pid) {
    pid->integral = 0.0;
    pid->previous_error = 0.0;
    pid->output = 0.0;
}